import numpy as np
import queue

# q3 = [[], [], []]
# q3x = queue.Queue(3)
# q3y = queue.Queue(3)
# q3z = queue.Queue(3)
# # q5 = [[], [], []]
# # q10 = [[], [], []]
# q5x = queue.Queue(5)
# q5y = queue.Queue(5)
# q5z = queue.Queue(5)
# q10x = queue.Queue(10)
# q10y = queue.Queue(10)
# q10z = queue.Queue(10)

# aver3, aver5, aver10 = 1.48, -0.19157, 8.3264

# aver3X, aver3Y, aver3Z = 0, 0, 0
# aver5X, aver5Y, aver5Z = 0, 0, 0
# aver10X, aver10Y, aver10Z = 0, 0, 0
# aver5 = -0.19157
# aver10 = 8.3264

def filterNum(num, acceX, acceY, acceZ, qx, qy, qz, averX, averY, averZ):
    if num == 3:
        avg3X, avg3Y, avg3Z, aver3X, aver3Y, aver3Z = filterAcce3(num, acceX, acceY, acceZ, qx, qy, qz, averX, averY, averZ)
        return avg3X, avg3Y, avg3Z, aver3X, aver3Y, aver3Z
    elif num == 5:
        avg5X, avg5Y, avg5Z, aver5X, aver5Y, aver5Z = filterAcce5(num, acceX, acceY, acceZ, qx, qy, qz, averX, averY, averZ)
        return avg5X, avg5Y, avg5Z, aver5X, aver5Y, aver5Z
    elif num == 10:
        avg10X, avg10Y, avg10Z, aver10X, aver10Y, aver10Z = filterAcce10(num, acceX, acceY, acceZ, qx, qy, qz, averX, averY, averZ)
        return avg10X, avg10Y, avg10Z, aver10X, aver10Y, aver10Z


def filterAcce3(num, acceX, acceY, acceZ, q3x, q3y, q3z, aver3X, aver3Y, aver3Z):

    if q3x.qsize() < 3:
        q3x.put(acceX)
        q3y.put(acceY)
        q3z.put(acceZ)
        aver3X += acceX
        aver3Y += acceY
        aver3Z += acceZ
        return acceX, acceY, acceZ, aver3X, aver3Y, aver3Z

    priorX = q3x.get()
    priorY = q3y.get()
    priorZ = q3z.get()
    q3x.put(acceX)
    q3y.put(acceY)
    q3z.put(acceZ)

    aver3X = (aver3X - priorX + acceX)
    aver3Y = (aver3Y - priorY + acceY)
    aver3Z = (aver3Z - priorZ + acceZ)
    return aver3X / 3, aver3Y / 3, aver3Z / 3, aver3X, aver3Y, aver3Z



def filterAcce5(num, acceX, acceY, acceZ, q5x, q5y, q5z, aver5X, aver5Y, aver5Z):

    if q5x.qsize() < 5:
        q5x.put(acceX)
        q5y.put(acceY)
        q5z.put(acceZ)
        aver5X += acceX
        aver5Y += acceY
        aver5Z += acceZ
        return acceX, acceY, acceZ, aver5X, aver5Y, aver5Z

    priorX = q5x.get()
    priorY = q5y.get()
    priorZ = q5z.get()
    q5x.put(acceX)
    q5y.put(acceY)
    q5z.put(acceZ)

    aver5X = (aver5X - priorX + acceX)
    aver5Y = (aver5Y - priorY + acceY)
    aver5Z = (aver5Z - priorZ + acceZ)
    return aver5X / 5, aver5Y / 5, aver5Z / 5, aver5X, aver5Y, aver5Z

def filterAcce10(num, acceX, acceY, acceZ, q10x, q10y, q10z, aver10X, aver10Y, aver10Z):

    if q10x.qsize() < 10:
        q10x.put(acceX)
        q10y.put(acceY)
        q10z.put(acceZ)
        aver10X += acceX
        aver10Y += acceY
        aver10Z += acceZ
        return acceX, acceY, acceZ, aver10X, aver10Y, aver10Z

    priorX = q10x.get()
    priorY = q10y.get()
    priorZ = q10z.get()
    q10x.put(acceX)
    q10y.put(acceY)
    q10z.put(acceZ)

    aver10X = (aver10X - priorX + acceX)
    aver10Y = (aver10Y - priorY + acceY)
    aver10Z = (aver10Z - priorZ + acceZ)
    return aver10X / 10, aver10Y / 10, aver10Z / 10, aver10X, aver10Y, aver10Z


